#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Esp32PcntEncoder.h>
#include <PidController.h> //引入pid控制器的头文件
#include <Kinematics.h>
#include <WiFi.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>
#include <nav_msgs/msg/odometry.h>
#include <micro_ros_utilities/string_utilities.h> //

rcl_publisher_t odom_publisher;
nav_msgs__msg__Odometry odom_msg;
rcl_timer_t timer;

rcl_subscription_t subscriber;     // 订阅者
geometry_msgs__msg__Twist sub_msg; // 存储订阅到的速度消息

/*声明相关的结构体对象*/
rcl_allocator_t allocator; // 内存分配器，用于动态内存分配管理
rclc_support_t support;    // 用于存储时钟、内存分配器和上下文，提供支持
rclc_executor_t executor;  // 执行器，用于管理订阅和计时器回调的执行
rcl_node_t node;           // 节点

Esp32PcntEncoder encoders[2];
Esp32McpwmMotor motor;
PidController pid_controller[2];
Kinematics kinematics;

float target_linear_speed = 50.0;  // mm/s
float target_angular_speed = 0.1f; // rad/s
float out_left_speed;
float out_right_speed;

int64_t last_ticks[2];
int32_t delta_ticks[2]; // 记录两次读取间的技术器差值
int64_t last_update_time;
float current_speed[2]; // 记录两个电动机的速度

// 在定时器回调函数中完成话题发布
void callback_publisher(rcl_timer_t *timer, int64_t last_call_time)
{
  odom_t odom = kinematics.get_odom();
  int64_t stamp = rmw_uros_epoch_millis(); // 获取当前时间
  odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000);
  odom_msg.header.stamp.nanosec = static_cast<int32_t>((stamp % 1000) * 1e6);
  odom_msg.pose.pose.position.x = odom.x;
  odom_msg.pose.pose.position.y = odom.y;
  odom_msg.pose.pose.orientation.w = cos(odom.angle * 0.5);
  odom_msg.pose.pose.orientation.x = 0;
  odom_msg.pose.pose.orientation.y = 0;
  odom_msg.pose.pose.orientation.z = sin(odom.angle * 0.5);
  odom_msg.twist.twist.angular.z = odom.angle_speed;
  odom_msg.twist.twist.linear.x = odom.linear_speed;

  // 发布里程计
  if (rcl_publish(&odom_publisher, &odom_msg, NULL) != RCL_RET_OK)
  {
    /* code */
    Serial.printf("error:odom publisher failed!\n");
  }
}

void twist_callback(const void *msg_in)
{
  // 将接收到的消息指针转化为geometry_msgs__msg_Twist类型
  const geometry_msgs__msg__Twist *twist_msg =
      (const geometry_msgs__msg__Twist *)msg_in;
  // 运动学逆解并设置速度
  kinematics.kinematic_inverse(twist_msg->linear.x * 1000,
                               twist_msg->angular.z, out_left_speed,
                               out_right_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
}

// 单独创建一个任务运行micro-ROS，相当于一个线程
void micro_ros_task(void *parameter)
{
  // 1.设置传输协议并延时等待设置完成
  IPAddress agent_ip;
  agent_ip.fromString("192.168.40.128");
  set_microros_wifi_transports("在这里填wife名称", "在这里填wife密码", agent_ip, 8888);
  delay(2000); // 等待网络设置完成
  // 2.初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 3.初始化support 函数会创建并初始化一个 rcl_context_t 结构体（包含在support中）
  rclc_support_init(&support, 0, NULL, &allocator);
  // 4.初始化节点
  rclc_node_init_default(&node, "fishbot_motion_control", "", &support);
  // 5.初始化执行器
  unsigned int num_handles = 2;
  rclc_executor_init(&executor, &support.context, num_handles, &allocator);
  // 6.初始化发布者
  odom_msg.header.frame_id = micro_ros_string_utilities_set(
      odom_msg.header.frame_id, "odom");
  odom_msg.child_frame_id = micro_ros_string_utilities_set(
      odom_msg.child_frame_id, "base_footprint");
  rclc_publisher_init_best_effort(
      &odom_publisher, &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/odom");
  // 7.时间同步
  while (!rmw_uros_epoch_synchronized())
  {
    /* code */
    Serial.printf("正在尝试时间同步");
    rmw_uros_sync_session(1000); // 尝试进行时间同步
    delay(10);
  }
  // 8.创建定时器，间隔50ms调用一次callback_publisher发布里程计话题
  rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(50), callback_publisher);
  rclc_executor_add_timer(&executor, &timer);
  // 初始化订阅者并添加到执行器中
  rclc_subscription_init_best_effort(
      &subscriber, &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg,
                                 &twist_callback, ON_NEW_DATA);
  // 循环执行器
  rclc_executor_spin(&executor);
}

// 自定义函数：
void motorSpeedController()
{
  uint64_t dt = millis() - last_update_time;
  delta_ticks[0] = encoders[0].getTicks() - last_ticks[0];
  delta_ticks[1] = encoders[1].getTicks() - last_ticks[1];
  current_speed[0] = float(delta_ticks[0] * 0.1051566) / dt * 1000;
  current_speed[1] = float(delta_ticks[1] * 0.1051566) / dt * 1000;
  last_update_time = millis();
  last_ticks[0] = encoders[0].getTicks();
  last_ticks[1] = encoders[1].getTicks();
  motor.updateMotorSpeed(0, pid_controller[0].update(current_speed[0]));
  motor.updateMotorSpeed(1, pid_controller[1].update(current_speed[1]));
}

void setup()
{
  // put your setup code here, to run once:
  // 创建任务运行 micro_ros_task
  xTaskCreate(micro_ros_task, // 任务函数
              "micro_ros",    // 任务名称
              1024,           // 任务堆栈大小
              NULL,           // 传递给任务函数的参数
              1,              // 任务优先级
              NULL            // 任务句柄
  );
  Serial.begin(115200);
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  motor.attachMotor(0, 22, 23);
  motor.attachMotor(1, 112, 13);

  // 初始化PID控制器参数
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);

  /*初始化轮子间距与电动机参数*/
  kinematics.set_wheel_distance(175);
  kinematics.set_motor_param(0, 0.1051566);
  kinematics.set_motor_param(1, 0.1051566);

  /*运动学逆解并设置速度*/
  kinematics.kinematic_inverse(target_linear_speed, target_angular_speed, out_left_speed, out_right_speed);

  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
}

void loop()
{
  // put your main code here, to run repeatedly:
  // 全局变量（定义在函数外的变量）：如果没有显式初始化，会被自动初始化为 “零值”（对整数类型就是 0，对指针就是 nullptr 等）。
  delay(10);
  kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks());
  motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0)));
  motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1)));
  Serial.printf("x=%f,y=%f,angle=%f\n", kinematics.get_odom().x, kinematics.get_odom().y,
                kinematics.get_odom().angle);
}